#include "Controller.h"
#include "../processing/processing.h"
#include "../utils/utils.hpp"
#include "../utils/pprint.hpp"
#include "../udp/udp.h"

using namespace std;

extern vector<Droplet> droplets;
char command[1024];
int index = 0;
int x, y;
const char END = -1;

SubController::SubController(std::vector<int>& route) {
    // Head* -> nullptr
    // Head -> [0]* -> nullptr
    // Head -> [0] -> [1]* -> nullptr
    // Head -> [0] -> [1] -> [2]* -> ... -> nullptr
    RouteNode* p = head;
    int num = (int)route.size();
    for (int i = 0; i < num - 1; ++i) {
        int& label = route[i];
        RouteNode* node = new RouteNode(label);
        p->next = node;
        p = node;
    }
    if (route.back() == route.front()) {
        p->next = head->next;
    }
    else {
        int& label = route.back();
        auto node = new RouteNode(label);
        p->next = node;
    }

    // print route list
    p = head;
    printf("  Planed route: ");
    for (int i = 0; i < 50 && p != nullptr; ++i) {
        printf("%d - ", p->label);
        p = p->next;
    }
    printf("\n");
}

SubController::~SubController() {
    RouteNode* p = head->next;
    while (p != nullptr) {
        head->next = p->next;
        delete p;
        p = head->next;
    }
    delete head;
}

void SubController::match(int label) {
    RouteNode* p = head->next;
    while (p != nullptr) {
        if (label == p->label) {
            this->currentNode = p;
            this->nextNode = p->next;
            return;
        }
        p = p->next;
    }
    throw string("Out of the route!");
}

void SubController::update(int currentLabel) {
    match(currentLabel);
    if (nextNode == nullptr) return;
    switch (state) {
    case STATE::Double:
        label2coor(currentNode->label, x, y);
        command[index++] = char(x);
        command[index++] = char(y);
        label2coor(nextNode->label, x, y);
        command[index++] = char(x);
        command[index++] = char(y);
        state = STATE::Single;
        break;

    case STATE::Single:
        label2coor(nextNode->label, x, y);
        command[index++] = char(x);
        command[index++] = char(y);
        state = STATE::Double;
        break;

    default:
        break;
    }
}

void Controller::init(vector<vector<int>>& routes) {
    printf("initController:\n");
    this->num = (uint)droplets.size();
    for (auto& droplet : droplets) {
        int label = getLabel(droplet.bbox);
        bool match = false;
        printf("  Droplet %d label [%d]  --->  ", droplet.id, label);
        for (auto& route : routes) {
            match = isExist(route, label);
            if (match) {
                cout << route << endl;
                this->subcontrollers[droplet.id] = new SubController(route);
                break;
            }
        }
        if (!match) {
            cerr << "Failed!" << endl;
            throw - 1;
        }
    }
}

void Controller::update() {
    index = 0;
    for (auto& droplet : droplets) {
        int label = getLabel(droplet.bbox);
        subcontrollers[droplet.id]->update(label);
    }
    command[index++] = END;
}

void Controller::sendCommand() {
    this->udp.send(command, index);
    printf("send command [%d]: ", index);
    for (int i = 0; command[i] != END; ++i) {
        printf("%d ", (int)command[i]);
    }
    printf("\n");
}
